#include "stm32f10x.h"
#include "stm32f10x_gpio.h"
#include "stm32f10x_rcc.h"
#include "pwm_motor.h"
#include "event.h"
#include "modbus.h"


void pwm_gpio_cfg()  
{    
    GPIO_InitTypeDef GPIO_InitStructure;    
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB|RCC_APB2Periph_GPIOE, ENABLE);    
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9 | GPIO_Pin_8;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;                                                  
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;     
    GPIO_Init(GPIOB,&GPIO_InitStructure);

    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1 | GPIO_Pin_0;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;                                                  
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;     
    GPIO_Init(GPIOE, &GPIO_InitStructure);
}


void motor_direction(MotorNumber motor_num, uint16_t dir)
{
    if (motor_num == kMotorOne){
        if(dir == 0) //clockwise
            GPIO_SetBits(GPIOE,GPIO_Pin_1);
        else
            GPIO_ResetBits(GPIOE,GPIO_Pin_1);
    } else if (motor_num == kMotorTwo) {
        if(dir == 0) //clockwise
            GPIO_SetBits(GPIOE,GPIO_Pin_0);
        else
            GPIO_ResetBits(GPIOE,GPIO_Pin_0);
    }
}

void timer4_init(uint16_t arr, uint16_t psc)    
{    
    TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;    
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);

    TIM_TimeBaseStructure.TIM_Prescaler =psc;     
    TIM_TimeBaseStructure.TIM_Period = arr;        
    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;        
    TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;        
    TIM_TimeBaseInit(TIM4,&TIM_TimeBaseStructure);  
}   

void pwm_out_config(MotorNumber motor_num)    
{    
    TIM_OCInitTypeDef TimOCInitStructure;    

    if (motor_num == kMotorOne){
        TimOCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;    
        TimOCInitStructure.TIM_Pulse = 20;     
        TimOCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;     
        TimOCInitStructure.TIM_OutputState = TIM_OutputState_Enable;        
        TIM_OC4Init(TIM4, & TimOCInitStructure);       
        TIM_OC4PreloadConfig(TIM4, TIM_OCPreload_Enable);
    } else if (motor_num == kMotorTwo){
        TimOCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;    
        TimOCInitStructure.TIM_Pulse = 20;     
        TimOCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;     
        TimOCInitStructure.TIM_OutputState = TIM_OutputState_Enable;        
        TIM_OC3Init(TIM4, & TimOCInitStructure);       
        TIM_OC3PreloadConfig(TIM4, TIM_OCPreload_Enable);  
    }
}

void pwm_out_disable(MotorNumber motor_num)    
{    
    TIM_OCInitTypeDef TimOCInitStructure;

    if (motor_num == kMotorOne){
        TimOCInitStructure.TIM_OutputState = TIM_OutputState_Disable;        
        TIM_OC4Init(TIM4, & TimOCInitStructure);   
    } else if (motor_num == kMotorTwo) {
        TimOCInitStructure.TIM_OutputState = TIM_OutputState_Disable;        
        TIM_OC3Init(TIM4, & TimOCInitStructure);   
    }
} 

void pwm_out_enable(MotorNumber motor_num)    
{    
    TIM_OCInitTypeDef TimOCInitStructure;    
            
    //TIM_ARRPreloadConfig(TIM4, ENABLE);
    if (motor_num == kMotorOne){
        TimOCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;    
        TimOCInitStructure.TIM_Pulse = PWM_A_DATA;     
        TimOCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;  
        TimOCInitStructure.TIM_OutputState = TIM_OutputState_Enable;        
        TIM_OC4Init(TIM4, & TimOCInitStructure);    
    } else if (motor_num == kMotorTwo) {
        TimOCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;    
        TimOCInitStructure.TIM_Pulse = PWM_B_DATA;     
        TimOCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;  
        TimOCInitStructure.TIM_OutputState = TIM_OutputState_Enable;        
        TIM_OC3Init(TIM4, & TimOCInitStructure);   
    }

} 


void pwm_init(void)   
{  
    pwm_gpio_cfg();  
    timer4_init(71,1000); //1k Hz
    pwm_out_config(kMotorOne);
    pwm_out_config(kMotorTwo);
    TIM_Cmd(TIM4, ENABLE);  
} 

void pwm_set_frq(void)
{  
    uint16_t prescaler;

    if(prescaler == 359)  
    {  
        prescaler=35;  
    } else {  
        prescaler=359;  
    }  
    TIM_PrescalerConfig(TIM4, prescaler,TIM_PSCReloadMode_Update);  
}
 
void pwm_set_duty(MotorNumber motor_num, uint16_t duty) // 10-1000  
{  
    if (motor_num == kMotorOne){
        TIM_SetCompare4(TIM4, duty);
    } else if (motor_num == kMotorTwo) {
        TIM_SetCompare3(TIM4, duty);  
    }
}

// enent handlers
void motor1_start(void)
{
    pwm_out_enable(kMotorOne);
}
void motor2_start(void)
{
    pwm_out_enable(kMotorTwo);
}

void motor1_stop(void)
{
    pwm_out_disable(kMotorOne);
}
void motor2_stop(void)
{
    pwm_out_disable(kMotorTwo);
}

void motor1_cw(void)
{
    motor_direction(kMotorOne, 0);
}
void motor2_cw(void)
{
    motor_direction(kMotorTwo, 0);
}
void motor1_ccw(void)
{
    motor_direction(kMotorOne, 1);
}
void motor2_ccw(void)
{
    motor_direction(kMotorTwo, 1);
}

void motor1_set_duty(void)
{
    if (PWM_A_DATA < 10) 
        PWM_A_DATA = 10;
    else if( PWM_A_DATA >= 500)
        PWM_A_DATA = 500;
    pwm_set_duty(kMotorOne, PWM_A_DATA);;
}

void motor2_set_duty(void)
{
    if (PWM_B_DATA < 10) 
        PWM_B_DATA = 10;
    else if( PWM_B_DATA >= 500)
        PWM_B_DATA = 500;

    pwm_set_duty(kMotorTwo, PWM_B_DATA);
}

void pwm_motor_event_handler_init(void)
{
    event_set_handler(kEventStartMotor1, motor1_start);
    event_set_handler(kEventStartMotor2, motor2_start);
    event_set_handler(kEventStopMotor1, motor1_stop);
    event_set_handler(kEventStopMotor2, motor2_stop);

    event_set_handler(kEventCWMotor1, motor1_cw);
    event_set_handler(kEventCCWMotor1, motor1_ccw);
    event_set_handler(kEventCWMotor2, motor2_cw);
    event_set_handler(kEventCCWMotor2, motor2_ccw);

    event_set_handler(kEventSetMotor1Duty, motor1_set_duty);
    event_set_handler(kEventSetMotor2Duty, motor2_set_duty);
}


